%% 分别使用EKF和UKF对基于伪距测量的滤波估计
% 状态方程为连续系统的线性方程，采用CV模型
% 量测方程为离散的非线性方程
clear all;
clc;

%% 参数初始化(CV模型)
T = 1;
matF = [zeros(4), zeros(4); zeros(4), eye(4)];
matPHI = [eye(4), eye(4)*T; zeros(4), eye(4)];
matG = [zeros(4); eye(4)];
matq = diag([0.01; 0.01; 0.01; 0.01]);
matQ = [eye(4)*(T^3/3)*matq, eye(4)*(T^2/2)*matq;
        eye(4)*(T^2/2)*matq, eye(4)*(T)*matq   ];
R = 3^2;

%% 真实数据与观测数据
realData = importdata('receiverMotion.txt');
measureData = importdata('measureData.txt');
% 生成真实位置速度
sizeReal = size(realData.data);
sampleNum = sizeReal(1);
realX = realData.data(:, 2:7)';
% 生成带噪声的伪距
rng(1);
measureZ = measureData.data(:,7) -...
            measureData.data(:,8) -...
            measureData.data(:,9) +...
            measureData.data(:,10);
sizeZ = size(measureZ);
rowsizeZ = sizeZ(1);
noiseZ = normrnd(0, sqrt(R), [rowsizeZ, 1]);
measureZ = measureZ + noiseZ;
% 初始数据
X0 = [realData.data(1,2:4), 0, realData.data(1,5:7), 0]';
%自己的程序起始P0较大时，起始估计较大抖动，而官方的不会
%P0 = diag(ones(8,1)*1e4);
P0 = inv(matPHI' * diag(ones(8,1)*R) * matPHI);

%% EKF
% 初值
estimateXpre = X0;
meanSquareErrorPre = P0;
indexZk = measureData.data(1, 2) + 1;
% 画图记录量
logXk_EKF = zeros(8, sampleNum);
logPk_EKF = zeros(8, sampleNum);
logXk_EKF(:, 1) = X0;
logPk_EKF(:, 1) = diag(P0);

for i = 2:1:sampleNum
    % 相对于KF，EKF的Phi阵要由状态方程f(X)实时求偏导得到
    matPHIk = matPHI;
    % 相对于KF，EKF的一部预测方程使用非线性的状态方程f(X)
    estimateXpre = matPHIk * estimateXpre;
    % 一步预测Pk
    meanSquareErrorPre = matPHIk*meanSquareErrorPre*matPHIk' + matQ;
    % 相对于KF，EKF的Hk阵要由观测方程h(X)实时求偏导得到
    svNum = measureData.data(indexZk, 2);
    svPos = measureData.data(indexZk:(indexZk+svNum-1), 4:6)';
    rcvPos = estimateXpre(1:3);
    matHk = getHk(svPos, rcvPos);
    matHkT = matHk';
    % 增益K
    matR = eye(svNum)*R;
    gainK = meanSquareErrorPre*matHkT/(matHk*meanSquareErrorPre*matHkT + matR);
    % X估计，相对于KF，此处使用观测方程h(X)
    hx = measurementFcn(estimateXpre, svPos);
    measureZk = measureZ(indexZk:(indexZk+svNum-1), 1);
    estimateX = estimateXpre + gainK*(measureZk - hx);
    % P估计
    tmp = eye(8) - gainK*matHk;
    meanSquareErrorK = tmp*meanSquareErrorPre*tmp' + gainK*matR*gainK';
    
    % 准备下一步
    estimateXpre = estimateX;
    meanSquareErrorPre = meanSquareErrorK;
    indexZk = indexZk + svNum;
    % 记录
    logXk_EKF(:, i) = estimateX;
    logPk_EKF(:, i) = diag(meanSquareErrorK);
    
end

%% UKF
% 初值
estimateXpre = X0;
meanSquareErrorPre = diag(ones(8,1)*1e4);
indexZk =  1;
% 画图记录量
logXk_UKF = zeros(8, sampleNum);
logPk_UKF = zeros(8, sampleNum);
logXk_UKF(:, 1) = X0;
logPk_UKF(:, 1) = diag(P0);

obj = unscentedKalmanFilter(@stateFcn,@measurementFcn,X0,'ProcessNoise',matQ,'MeasurementNoise',R);

for k = 1:1:sampleNum
    svNum = measureData.data(indexZk,2);
    svPos = measureData.data(indexZk:(indexZk+svNum-1), 4:6)';
    % 先correct后prdecit, 记录k而不是k+1
    [CorrectedState, CorrectedStateCovariance] = correct(obj, measureZ(indexZk:(indexZk+svNum-1)), svPos);
    [PredictedState, PredictedStateCovariance] = predict(obj, matPHI);
    indexZk = indexZk + svNum;
    
    logXk_UKF(:, k) = CorrectedState;
    logPk_UKF(:, k) = diag(CorrectedStateCovariance);
end

%% 绘图
t = 0:1:sampleNum-1;

figure;
subplot(211);
plot(t, logXk_EKF(1:3,:)-realX(1:3,:));
grid on;
title('EKF位置估计误差');
ylabel('位置误差(m)');
legend('X','Y','Z');
subplot(212);
plot(t, logXk_EKF(5:7,:)-realX(4:6,:));
grid on;
title('EKF速度估计误差');
ylabel('速度误差（m/s）');
legend('vx','vy','vz');

figure;
subplot(211);
plot(t, logXk_UKF(1:3,:)-realX(1:3,:));
grid on;
title('UKF位置估计误差');
ylabel('位置误差(m)');
legend('X','Y','Z');
subplot(212);
plot(t, logXk_UKF(5:7,:)-realX(4:6,:));
grid on;
title('UKF速度估计误差');
ylabel('速度误差（m/s）');
legend('vx','vy','vz');

%% 小函数
function y = getHk(s, rc)
    svNum = length(s);
    y = zeros(svNum, 8);
    for i = 1:1:svNum
        y(i, 1:3) = (rc-s(:,i))' / norm(rc-s(:,i));
        y(i, 4) = 1;
    end
end

function y = measurementFcn(x, s)
    svNum = length(s);
    rc = x(1:3);
    clockerror = x(4);
    y = zeros(svNum, 1);
    for i = 1:1:svNum
        y(i) = norm(rc-s(:,i)) + clockerror;
    end
end

function y = stateFcn(x, phi)
    y = phi*x;
end
